#include "fuzzy_pid.h"

#include <stdio.h>  // 确保包含标准IO头文件
#include <unistd.h>  // 用于usleep函数

// 批量pid个数
#define DOF 6

// 模糊集合、隶属函数、模糊推理

void fuzzy_vector_init_for_pwd_use(void){
    // 1.定义挡位
    // FUZZY_PID_QF_DEFAULT 

    // 3. delta kp/ki/kd 的默认模糊规则库
    // 根据误差（e）和误差变化率（ec）决定参数调整量
    /*
    ​​模糊集合​​：
    用​​隶属度​​（0到1之间的数值）表示温度属于“高”的程度。例如：
    25℃ → 隶属度0.2（轻微属于）
    30℃ → 隶属度0.5（中等属于）
    35℃ → 隶属度1（完全属于）
    这样能更自然地表达“逐渐变热”的过程（参考）。
 
    > 说明
    ​​行维度​​：表示​​当前 误差等级​​（从PB到NB共7级） ！！！
    ​​列维度​​：表示​​误差 变化率 等级​​（从PB到NB共7级） ！！！

    > 规则
    ​​ΔKp规则​​：误差越大，调整幅度越强（PB/PB/PM），快速抑制偏差；
    ​​ΔKi规则​​：误差反向时（NM），需抑制积分作用（NB/NM），防止超调；
    ​​ΔKd规则​​：误差剧烈变化时（EC=PB/PS），通过正/负微分抑制振荡（PS/NS/NB）。

    当误差极大（PB）且误差变化率极大（PB）时，需​​大幅增强比例作用​​（ΔKp=+3）以快速抑制偏差。
    ​​完全关闭积分作用​​（ΔKi=-3）避免超调，并通过​​小幅增强微分作用​​（ΔKd=+1）抑制振荡。

    */
    int rule_base[][FUZZY_PID_QF_DEFAULT] = {
            // delta kp 规则库（7x7）
            // kp默认参数
            {PB, PB, PM, PM, PS, ZO, ZO},
            {PB, PB, PM, PS, PS, ZO, NS},
            {PM, PM, PM, PS, ZO, NS, NS},
            {PM, PM, PS, ZO, NS, NM, NM},
            {PS, PS, ZO, NS, NS, NM, NM},
            {PS, ZO, NS, NM, NM, NM, NB},
            {ZO, ZO, NM, NM, NM, NB, NB},

            //delta ki rule base
            {NB, NB, NM, NM, NS, ZO, ZO},
            {NB, NB, NM, NS, NS, ZO, ZO},
            {NB, NM, NS, NS, ZO, PS, PS},
            {NM, NM, NS, ZO, PS, PM, PM},
            {NM, NS, ZO, PS, PS, PM, PB},
            {ZO, ZO, PS, PS, PM, PB, PB},
            {ZO, ZO, PS, PM, PM, PB, PB},

            //delta kd rule base
            {PS, NS, NB, NB, NB, NM, PS},
            {PS, NS, NB, NM, NM, NS, ZO},
            {ZO, NS, NM, NM, NS, NS, ZO},
            {ZO, ZO, ZO, ZO, ZO, ZO, ZO},
            {PS, PS, PM, PM, ZO, ZO, ZO},
            {PB, PS, PS, PS, PS, PS, PB},
            {PB, PM, PM, PM, PS, PS, PB}
            
    };

    //2. Default parameters of membership function
    // 隶属度函数（Membership Function） 0-1
    // 每个模糊子集对应的“隶属度曲线”，表示某个数值属于该模糊语言的程度。
    /*
    动态控制          
    0 高斯隶属度        
    其余 三角形隶属度

    风险评估
    2 S形隶属度函数 
    3 梯形隶属度函数
    
    图形处理
    1 广义钟形隶属度函数
    5 Z形隶属度函数
    */
    int mf_params[4 * FUZZY_PID_QF_DEFAULT] = {-3, -2, -2, 0,
                                    // -3, -2, -2, 0  表示三角形顶点在 第一个第三个俩个点 -3 -2 ，中间的点第二个-3
                                     -3, -2, -1, 0,
                                     -2,  0,  0, 0,
                                      0,  0,  1, 0,
                                      1,  1,  2, 0,
                                      1,  2,  3, 0,
                                      1,  3,  3, 0};

    // pid 控制器的默认参数
                                            // Kp, Ki,Kd,   intergral_limit,dead_zone,feed_forward
                                            //    原始pid值  积分限制幅度 死区阈值 前馈补偿调节
    float fuzzy_pid_params[DOF][FUZZY_PID_PARAMS_COUNT] = {
                                                     {18.0f,0.006f,0.1f,   0, 0, 0, 1},
                                                     {-0.34f, 0,     0,    0, 0, 0, 1}, // 修改
                                                     {-0.4f,  0,     0,    0, 0, 0, 1},
                                                     {-0.4f,  0,     0,    0, 0, 0, 1},
                                                     {0.2f,   0,     0,    0, 0, 0, 1},
                                                     {10.0f,0.006f, 0.1f,  0, 0, 0, 1}};

    // 根据参数获取 PID 控制器向量
    struct PID **pid_vector = fuzzy_pid_vector_init(fuzzy_pid_params, 2.0f, 4, 1, 0, mf_params, rule_base, DOF);



    printf("output:\n");
    int control_id = 0;
    float real = 20;
    float idea = MAX_ERROR * 0.9f;
    printf("idea value: %f\n", idea);
    bool direct[DOF] = {true, false, false, false, true, true};

    // 存储 数据
    // 打开文件（如果不存在则创建，存在则覆盖）
    FILE *file = fopen("test.csv", "w");
    if (file == NULL) {
        printf("无法创建文件！\n");
        // return -1;
        return;
    }
    int count = 0;
    for (int j = 0; j < 200; ++j) {
        int out = fuzzy_pid_motor_pwd_output(real, idea, direct[control_id], pid_vector[control_id]);
        real += (float) (out - FUZZY_PID_MIDDLE_PWM_OUTPUT) / (float) FUZZY_PID_MIDDLE_PWM_OUTPUT * (float) MAX_ERROR * 0.6f;
        // real

        // real += ((rand()%21)-10)*0.04f; 
        // ±0.4°C噪声
        // printf("%d,%f\n", out, real);
        
        // if(real == idea){
        //     count++;
        //     if(count==2){
        //         idea = 1.5f*idea;
        //         count = 0;
        //     }
        // }

        // 将数据写入文件（格式：out,real）
        fprintf(file, "%d,%f\n", out, real);
    }

    // 关闭文件
    fclose(file);
    printf("数据已保存到 test.csv\n");

    delete_pid_vector(pid_vector, DOF);
}


void fuzzy_init_for_pwd_use(void){
    
    int rule_base[][FUZZY_PID_QF_DEFAULT] = {
            // delta kp 规则库（7x7）
            // kp默认参数
            {PB, PB, PM, PM, PS, ZO, ZO},
            {PB, PB, PM, PS, PS, ZO, NS},
            {PM, PM, PM, PS, ZO, NS, NS},
            {PM, PM, PS, ZO, NS, NM, NM},
            {PS, PS, ZO, NS, NS, NM, NM},
            {PS, ZO, NS, NM, NM, NM, NB},
            {ZO, ZO, NM, NM, NM, NB, NB},

            //delta ki rule base
            {NB, NB, NM, NM, NS, ZO, ZO},
            {NB, NB, NM, NS, NS, ZO, ZO},
            {NB, NM, NS, NS, ZO, PS, PS},
            {NM, NM, NS, ZO, PS, PM, PM},
            {NM, NS, ZO, PS, PS, PM, PB},
            {ZO, ZO, PS, PS, PM, PB, PB},
            {ZO, ZO, PS, PM, PM, PB, PB},

            //delta kd rule base
            {PS, NS, NB, NB, NB, NM, PS},
            {PS, NS, NB, NM, NM, NS, ZO},
            {ZO, NS, NM, NM, NS, NS, ZO},
            {ZO, ZO, ZO, ZO, ZO, ZO, ZO},
            {PS, PS, PM, PM, ZO, ZO, ZO},
            {PB, PS, PS, PS, PS, PS, PB},
            {PB, PM, PM, PM, PS, PS, PB}
    };

    //2. Default parameters of membership function
    // 隶属度函数（Membership Function） 0-1
    int mf_params[4 * FUZZY_PID_QF_DEFAULT] = {-3, -2, -2, 0,
                                               -3, -2, -1, 0,
                                               -1,  0,  0, 0,
                                                0,  0,  1, 0,
                                                1,  1,  2, 0,
                                                1,  2,  3, 0,
                                                1,  3,  3, 0};

    // pid 控制器的默认参数
                                            // Kp, Ki,Kd,   intergral_limit,dead_zone,feed_forward
                                            //    原始pid值  积分限制幅度 死区阈值 前馈补偿调节

    float fuzzy_pid_params[FUZZY_PID_PARAMS_COUNT] = {18.0f,0.006f,0.1f,   0, 0.2f, 0, 1};

    // 根据参数获取 PID 控制器向量
    //   1 隶属度的乘积
    struct PID *pid_vector = fuzzy_pid_init(fuzzy_pid_params, 2.0f, Trimf, 1, 0, mf_params, rule_base);
    
    float real = 40;
    float idea = MAX_ERROR * 0.9f;
    printf("idea value: %f\n\n", idea);

    // 存储 数据
    // 打开文件（如果不存在则创建，存在则覆盖）
    FILE *file = fopen("test.csv", "w");
    if (file == NULL) {
        printf("无法创建文件！\n");
        // return -1;
        fclose(file);
        delete_pid(pid_vector);
        return;
    }

    for (int j = 0; j < 13; ++j) {
        
        printf("\ncount: %d\n",j);
        int out = fuzzy_pid_motor_pwd_output(real, idea, true, pid_vector);
        real += (float) (out - FUZZY_PID_MIDDLE_PWM_OUTPUT) / (float) FUZZY_PID_MIDDLE_PWM_OUTPUT * (float) MAX_ERROR * 0.1f;
        
        // printf("real:%f,error:%f",real,idea-real);
        // real += ((rand()%21)-10)*0.1f; 

        // ±0.4°C噪声
        // printf("%d,%f\n", out, real);

        // 将数据写入文件（格式：out,real）
        fprintf(file, "%d,%f\n", out, real);
    }

    // 关闭文件
    fclose(file);
    printf("\n\n数据已保存到 test.csv\n");

    delete_pid(pid_vector);
}

/**
 * @brief 
 * 
 */
void fuzzy_init_for_ctotal_use(void){
    int rule_base[][FUZZY_PID_QF_DEFAULT] = {
            // delta kp 规则库（7x7）
            // kp默认参数
            {PB, PB, PM, PM, PS, ZO, ZO},
            {PB, PB, PM, PS, PS, ZO, NS},
            {PM, PM, PM, PS, ZO, NS, NS},
            {PM, PM, PS, ZO, NS, NM, NM},
            {PS, PS, ZO, NS, NS, NM, NM},
            {PS, ZO, NS, NM, NM, NM, NB},
            {ZO, ZO, NM, NM, NM, NB, NB},

            //delta ki rule base
            {NB, NB, NM, NM, NS, ZO, ZO},
            {NB, NB, NM, NS, NS, ZO, ZO},
            {NB, NM, NS, NS, ZO, PS, PS},
            {NM, NM, NS, ZO, PS, PM, PM},
            {NM, NS, ZO, PS, PS, PM, PB},
            {ZO, ZO, PS, PS, PM, PB, PB},
            {ZO, ZO, PS, PM, PM, PB, PB},

            //delta kd rule base
            {PS, NS, NB, NB, NB, NM, PS},
            {PS, NS, NB, NM, NM, NS, ZO},
            {ZO, NS, NM, NM, NS, NS, ZO},
            {ZO, ZO, ZO, ZO, ZO, ZO, ZO},
            {PS, PS, PM, PM, ZO, ZO, ZO},
            {PB, PS, PS, PS, PS, PS, PB},
            {PB, PM, PM, PM, PS, PS, PB}
    };

    //2. Default parameters of membership function
    // 隶属度函数（Membership Function） 0-1
    int mf_params[4 * FUZZY_PID_QF_DEFAULT] = {-3, -2, -2, 0,
                                     -3, -2, -1, 0,
                                     -1,  0,  0, 0,
                                      0,  0,  1, 0,
                                      1,  1,  2, 0,
                                      1,  2,  3, 0,
                                      1,  3,  3, 0};

    // pid 控制器的默认参数
                                            // Kp, Ki,Kd,   intergral_limit,dead_zone,feed_forward
                                            //    原始pid值  积分限制幅度 死区阈值 前馈补偿调节

    float fuzzy_pid_params[FUZZY_PID_PARAMS_COUNT] = {18.0f,0.006f,0.1f,   0, 0, 0, 1};

    // 根据参数获取 PID 控制器向量
    struct PID *pid_vector = fuzzy_pid_init(fuzzy_pid_params, 2.0f, Trimf, 1, 0, mf_params, rule_base);
    
    float real = 30;
    float idea = MAX_ERROR * 0.9f;
    printf("idea value: %f\n", idea);

    // 存储 数据
    // 打开文件（如果不存在则创建，存在则覆盖）
    FILE *file = fopen("test.csv", "w");
    if (file == NULL) {
        printf("无法创建文件！\n");
        // return -1;
        fclose(file);
        delete_pid(pid_vector);
        return;
    }

    for (int j = 0; j < 200; ++j) {
        float out = fuzzy_pid_control(real, idea, pid_vector);
        real +=out;
        // int out = fuzzy_pid_motor_pwd_output(real, idea, true, pid_vector);
        // real += (float) (out - FUZZY_PID_MIDDLE_PWM_OUTPUT) / (float) FUZZY_PID_MIDDLE_PWM_OUTPUT * (float) MAX_ERROR * 0.1f;


        if(j%2==0){
            real += ((rand()%21)-10)*0.05f; 
        }

        // ±0.4°C噪声
        // printf("%d,%f\n", out, real);

        // 将数据写入文件（格式：out,real）
        fprintf(file, "%f,%f\n", out, real);
    }

    // 关闭文件
    fclose(file);
    printf("数据已保存到 test.csv\n");

    delete_pid(pid_vector);
}


int main() {
    // fuzzy_vector_init_for_pwd_use();
    fuzzy_init_for_pwd_use();
    // fuzzy_init_for_ctotal_use();
    return 0;
}